/* USER CODE BEGIN Header */
/**
  ******************************************************************************
  * File Name          : freertos.c
  * Description        : Code for freertos applications
  ******************************************************************************
  * @attention
  *
  * Copyright (c) 2025 STMicroelectronics.
  * All rights reserved.
  *
  * This software is licensed under terms that can be found in the LICENSE file
  * in the root directory of this software component.
  * If no LICENSE file comes with this software, it is provided AS-IS.
  *
  ******************************************************************************
  */
/* USER CODE END Header */

/* Includes ------------------------------------------------------------------*/
#include "FreeRTOS.h"
#include "task.h"
#include "main.h"

/* Private includes ----------------------------------------------------------*/
/* USER CODE BEGIN Includes */
#include "mpu6050.h"
#include "user_freertos.h"
#include "queue.h"
#include "PID.h"
#include <stdio.h>
#include "Motor.h"
#include "stm32f1xx_hal_tim.h"
/* USER CODE END Includes */

/* Private typedef -----------------------------------------------------------*/
/* USER CODE BEGIN PTD */

/* USER CODE END PTD */

/* Private define ------------------------------------------------------------*/
/* USER CODE BEGIN PD */

/* USER CODE END PD */

/* Private macro -------------------------------------------------------------*/
/* USER CODE BEGIN PM */

/* USER CODE END PM */

/* Private variables ---------------------------------------------------------*/
/* USER CODE BEGIN Variables */

PID_TypeDef speed_outer = {

	.target=0,

	.Kp=0.12,
	.Ki=0.045,
	.Kd=0,

	.out_upper_limit=100,
	.out_lower_limit=-100,

	.sum_lower_limit=-50,
	.sum_upper_limit=50,


};


PID_TypeDef degree_inner = {

	.target=0,

	.Kp=4.3,
	.Ki=0,
	.Kd=0.2,

	.out_upper_limit=100,
	.out_lower_limit=-100,

	.sum_lower_limit=-300,
	.sum_upper_limit=300,


};

osMutexId_t target_degree_mutex,current_location_mutex,target_speed_mutex,pitch_roll_mutex;

osMessageQueueId_t mpu6050_euler_angles_QueueHandle;

extern TIM_HandleTypeDef  htim3,htim4 ;


float pitch=0,roll=0;

/* USER CODE END Variables */

/* Private function prototypes -----------------------------------------------*/
/* USER CODE BEGIN FunctionPrototypes */
extern PID_TypeDef location_pid;
/* USER CODE END FunctionPrototypes */

/* Private application code --------------------------------------------------*/
/* USER CODE BEGIN Application */

void init_mutex_queue_etc(void){



	const osMessageQueueAttr_t mpu6050_euler_angles_attributes = {
	  .name = "mpu6050_raw_data"
	};

	target_degree_mutex   =  osMutexNew(NULL);

	current_location_mutex   =  osMutexNew(NULL);

	pitch_roll_mutex   =  osMutexNew(NULL);

	target_speed_mutex   =  osMutexNew(NULL);

	mpu6050_euler_angles_QueueHandle = osMessageQueueNew (4, sizeof(float), &mpu6050_euler_angles_attributes);

}

void read_mpu6050_data_Task(void *argument)
{


	init_mpu6050();

	const TickType_t xMaxBlockTime = pdMS_TO_TICKS( 10 );

  /* Infinite loop */
  for(;;)
  {
	uint32_t current_tick =   osKernelGetTickCount();

	// 1、通过dma和dma传输完成中断 发起对mpu6050 加速度计和陀螺仪寄存器的读取操作
	init_read_accel_gyro();

    /*
     * 2、阻塞当前task，等待dma传输完成
     * Wait for the transmission to complete. */
	uint32_t  ulNotificationValue = ulTaskNotifyTake( pdFALSE, xMaxBlockTime );



    if( ulNotificationValue == 0 )
    {
        /*
         * 重试
         * The call to ulTaskNotifyTake() timed out. */
    	continue;
    }

	/*
	 * 3、dma数据传输完成，当前task被唤醒解析欧拉角
	 * The transmission ended as expected.
	 * */
    Euler_angles euler_angles = calculate_euler_angles();

    /*
     * 4、把计算结果（roll）放入阻塞队列
     * */
    //xQueueSend( mpu6050_euler_angles_QueueHandle,&euler_angles.roll,10);

	if(osMutexAcquire(pitch_roll_mutex,2) == osOK){

		roll =  euler_angles.roll;
		pitch = euler_angles.pitch;

		osMutexRelease(pitch_roll_mutex);
	}




	// 每10个tick执行一次
	osDelayUntil(current_tick + 10);
  }

}

float target_degree = 0;

/**栈溢出（Stack Overflow）
 * 现象：任务栈不足时，可能破坏其他任务或内核数据，触发 HardFault。
 * 注意：sprintf 和浮点运算（%f）可能消耗大量栈空间。
 */
void print_debug_by_uart_task(void * argument){

	UART_HandleTypeDef * huart = (UART_HandleTypeDef *)argument;

	uint8_t  str[150];

	while(1){

		uint32_t current_tick =   osKernelGetTickCount();

		UBaseType_t highWaterMark = uxTaskGetStackHighWaterMark(NULL);

		size_t mem = xPortGetFreeHeapSize();

		int length=0;

		float td=0;

		if(osMutexAcquire(target_degree_mutex,2) == osOK){

			td= target_degree;


			osMutexRelease(target_degree_mutex);
		}else{
			continue;
		}


		if(osMutexAcquire(pitch_roll_mutex,2) == osOK){

			float p = pitch;
			float r = roll;

			osMutexRelease(pitch_roll_mutex);

			length= sprintf((char *)str,"%f,%f,%f,%l,%u\r\n",p,r,td,highWaterMark,mem);

		}else{
			continue;
		}


		  HAL_UART_Transmit_DMA(huart,str,length);

		  // 每100ms发送一次串口调试数据
		  osDelayUntil(current_tick + 100);
	}
}




void degree_pid_task(void * argument){



	TIM_HandleTypeDef * htim = (TIM_HandleTypeDef *) argument ;



	while(1){

		uint32_t current_tick =   osKernelGetTickCount();

		/*
		 * The value is read into a struct AMessage variable, so after calling
           xQueueReceive() xRxedStructure will hold a copy of xMessage
		 * */
		/*if(xQueueReceive(mpu6050_euler_angles_QueueHandle,&roll,10)==pdFALSE){
			// 从队列中接收数据失败
			continue;
		}*/

		if(osMutexAcquire(pitch_roll_mutex,2) == osOK){


			degree_inner.current =  roll;

			osMutexRelease(pitch_roll_mutex);


		}else{
			continue;
		}




	  float out_duty =0;

	  if(degree_inner.current>45||degree_inner.current<-45){

		  degree_inner.error_sum=0;
		  // 俯仰角倾斜角度过大，电机停止旋转
	  }else{

			if(osMutexAcquire(target_degree_mutex,2) == osOK){

				 degree_inner.target =  target_degree;

				osMutexRelease(target_degree_mutex);
			}


		  out_duty = pid(&degree_inner);

	  }



	  set_pwm_right(htim,out_duty);
	  set_pwm_left(htim,out_duty);


	  // 每10ms发送一次串口调试数据
	  osDelayUntil(current_tick + 10);
	}

}


int32_t current_location=0,target_speed=0;

/**
 * 需传入定时器3的指针(htim3,htim4)
 */
void speed_pid_task(void * argument){

	int16_t left_prev=0,right_prev=0;

	while(1){

		uint32_t current_tick =   osKernelGetTickCount();

		int16_t current_count_left = __HAL_TIM_GET_COUNTER(&htim3);

		int16_t d_location_left = read_encoder_delta_location(&left_prev,current_count_left);

		int16_t current_count_right = __HAL_TIM_GET_COUNTER(&htim4);

		int16_t d_location_right = read_encoder_delta_location(&right_prev,current_count_right);

		speed_outer.current = -(d_location_left+d_location_right)/2.0;



		if(osMutexAcquire(current_location_mutex,2) == osOK){

			current_location += speed_outer.current;

			osMutexRelease(current_location_mutex);
		}else{
			continue;
		}


		if(osMutexAcquire(target_speed_mutex,2) == osOK){

			speed_outer.target  = target_speed;

			osMutexRelease(target_speed_mutex);
		}else{
			continue;
		}





		float output = pid(&speed_outer);

		if(osMutexAcquire(target_degree_mutex,2) == osOK){

			target_degree = output;

			osMutexRelease(target_degree_mutex);
		}else{
			continue;
		}

		// 每50ms执行一次
		osDelayUntil(current_tick + 50);
	}
}

void location_pid_task(void * argument){

	while(1){

		uint32_t current_tick =   osKernelGetTickCount();

		if(osMutexAcquire(current_location_mutex,2) == osOK){

			location_pid.current = -current_location;

			osMutexRelease(current_location_mutex);
		}

		float output = pid(&location_pid);


		if(osMutexAcquire(target_speed_mutex,2) == osOK){

			target_speed  =output;

			osMutexRelease(target_speed_mutex);
		}



		// 每100ms执行一次
		osDelayUntil(current_tick + 100);
	}
}

/* USER CODE END Application */

